#include "MeshObject.h"

#include <iostream>
MeshObject::MeshObject(std::vector<ColorTriangle>& vect, Vertex3D pos, Vertex3D vPos, Vertex3D rot, Vertex3D vRot, PhysicsModel physModel) : Object(pos, vPos, rot, vRot)
{
    for (int i = 0; i < vect.size(); i++)
    {
        ColorTriangle a = vect[i];
        mesh.push_back(a);
    }
    physicsModel = physModel;
}
void MeshObject::setPhysics(PhysicsModel model)
{
    this->physicsModel = model;
}
MeshObject::~MeshObject()
{
    //dtor
}

void MeshObject::update()
{
    updatePos();
}

void MeshObject::collide(MeshObject* a)
{
    if (this->physicsModel == NONE || a->physicsModel == NONE) return;
    if (this->physicsModel == SPHERE && a->physicsModel == SPHERE)
    {
        Vertex3D dist = this->pos - a->pos;
        if (dist.getR() <= (this->radius + a->radius) )
        {
            Vertex3D e = dist/dist.getR();
            this->vPos += e * (this->vPos*e);
            a->vPos -= e * (a->vPos*e);
            while (dist.getR() <=this->radius + a->radius)
            {
                this->pos += e*0.01;
                a->pos -= e*0.01;
                dist = this->pos - a->pos;
            }
        }
        else
        {
            return;
        }
    }
    if (this->physicsModel == TRIANGLES && a->physicsModel == SPHERE)
    {
        for (int i = 0; i < mesh.size(); i++)
        {
            //Check parallel and behind;
            if (this->mesh[i].getNormal() * a->vPos == 0) continue;

            Vertex3D massCenter = this->pos + (this->mesh[i].Getpoint(0) + this->mesh[i].Getpoint(1) + this->mesh[i].Getpoint(2))/3;

            double b = (this->mesh[i].getNormal() * (a->vPos / a->vPos.getR() ) );
            double t = this->mesh[i].getNormal() * (massCenter - a->pos);
            t /= b;

            //std::cout << t << std::endl;

            if (t < 0 ) continue;

            if (t <= a->radius)
            {
                //We've collided /w plane!
                //Now check if in triangle.
                /*Vertex3D z = massCenter - a->pos;
                double h = z * this->mesh[i].getNormal();
                Vertex3D proj = z - h*this->mesh[i].getNormal();
                Vertex3D projected_point = z - h*this->mesh[i].getNormal();

                bool check1 = false;
                Vertex3D cp1 = CrossProduct(this->mesh[i].Getpoint(2) - this->mesh[i].Getpoint(1), projected_point - this->mesh[i].Getpoint(1));
                Vertex3D cp2 = CrossProduct(this->mesh[i].Getpoint(2) - this->mesh[i].Getpoint(1), this->mesh[i].Getpoint(0) - this->mesh[i].Getpoint(1));
                if (cp1 * cp2 >= 0) check1 = true;

                bool check2 = false;
                Vertex3D cp3 = CrossProduct(this->mesh[i].Getpoint(2) - this->mesh[i].Getpoint(0), projected_point - this->mesh[i].Getpoint(0));
                Vertex3D cp4 = CrossProduct(this->mesh[i].Getpoint(2) - this->mesh[i].Getpoint(0), this->mesh[i].Getpoint(1) - this->mesh[i].Getpoint(0));
                if (cp3 * cp4 >= 0) check2 = true;

                bool check3 = false;
                Vertex3D cp5 = CrossProduct(this->mesh[i].Getpoint(1) - this->mesh[i].Getpoint(0), projected_point - this->mesh[i].Getpoint(0));
                Vertex3D cp6 = CrossProduct(this->mesh[i].Getpoint(1) - this->mesh[i].Getpoint(0), this->mesh[i].Getpoint(2) - this->mesh[i].Getpoint(0));
                if (cp5 * cp6 >= 0) check3 = true;

                if (check1 && check2 && check3)
                {*/
                    Vertex3D temp = this->mesh[i].getNormal() * (-(a->vPos) * this->mesh[i].getNormal() ) ;
                    temp *= 2;
                    a->vPos += temp;
                    a->vPos *= 0.7;
                    if (a->vPos.getR() < 0.002)
                    {
                        a->vPos = Vertex3D(0,0,0);

                        std::cout << "Stopped.\n";
                    }
                    else
                    {
                        std::cout<< "Collision at:" << a->pos.Getx() <<"," << a->pos.Gety() <<"," << a->pos.Getz() <<","  <<" \n";
                    }
                    break;
                //}

            }
        }
    }
}
void MeshObject::draw()
{
    glPushMatrix();

    glRotatef(rot.Getx(), 1,0,0);
    glRotatef(rot.Gety(), 0,1,0);
    glRotatef(rot.Getz(), 0,0,1);
    glTranslatef(pos.Getx(),pos.Gety(),pos.Getz());

    glBegin(GL_TRIANGLES);

    for (int i = 0; i < mesh.size(); i++)
    {
        for (int j = 0; j < 3; j++)
        {
            glColor3f(mesh[i].Getcolor(j).Getx(),mesh[i].Getcolor(j).Gety(),mesh[i].Getcolor(j).Getz());
            glVertex3f(mesh[i].Getpoint(j).Getx(),mesh[i].Getpoint(j).Gety(),mesh[i].Getpoint(j).Getz());
        }
    }

    glEnd();
    glPopMatrix();
}
//Load Wavefrom Obj model
MeshObject::MeshObject(std::string filename, PhysicsModel physModel) : Object(Vertex3D())
{
    physicsModel = physModel;
    std::ifstream file;
    file.open(filename.c_str());
    if (!file.is_open()) return;
    std::vector<Vertex3D> verts;
    std::vector<ColorTriangle> tris;
    int color = 0;
    std::vector<std::string> lines;
    while (!file.eof())
    {
        char line[256];
        file.getline(line, 256);
        lines.push_back(std::string(line));
    }
    for(int i = 0; i < lines.size(); i++)
    {
        double max= 0, min =0;
        if (lines[i][0] == 'v')
        {
            std::stringstream asdf;
            asdf.str(lines[i]);
            char a; double b,c,d;
            asdf >> a >> b >> c >>d;
            verts.push_back(Vertex3D(b,c,d));
            if (c > max) max = c;
            if (c < min) min = c;
        }
        if (lines[i][0] == 'f')
        {
            int vert[3];
            std::stringstream asdf;
            asdf.str(lines[i]);
            char d;
            std::string a[3];
            asdf >> d >> a[0] >> a[1] >>a[2];
            //One
            for (int j =0 ; j<3;j++)
            {
                std::stringstream temp;
                temp.str(a[j]);
                std::string lulz;

                temp.get(d);
                while (d != '/' && d != ' ' && temp.good() )
                {
                    lulz += d;
                    temp.get(d);
                }
                std::stringstream foobar;
                foobar.str(lulz);
                int asdfjkl;
                foobar >> asdfjkl;
                vert[j] = --asdfjkl;
            }

            tris.push_back(ColorTriangle(verts[vert[0]],verts[vert[1]],verts[vert[2]], /*ColorVertex(verts[vert[0]].Gety()/(max-min),1,1), ColorVertex( verts[vert[1]].Gety()/(max-min), 1, 1 ), ColorVertex(verts[vert[2]].Gety()/(max-min),1,1)*/ ColorVertex(1,0,0),ColorVertex(0,1,0),ColorVertex(0,0,1)));
        }
    }
    this->mesh = tris;
}
